// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DriveFeedbackState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
#define USE_HEADING_WITH_WHEEL_NOISE

namespace Microsoft.Robotics.Hardware.DAC
{
    using System;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Algorithms;
    using Microsoft.Robotics.Numerics;
    using MicrosoftResearch.Infer.Distributions;

    /// <summary>
    /// Contains state elements for GenericDriveState derived from hardware feedback.
    /// </summary>
    [DataContract]
    public class DriveFeedbackState : IDriveState
    {
        /// <summary>
        /// Distance between the wheels in meters.
        /// </summary>
        [DataMember]
        private double wheelbase;

        /// <summary>
        /// Mean distance travelled of the right wheel in meters
        /// </summary>
        [DataMember]
        private double rightWheelMean;

        /// <summary>
        /// Mean  distance travelled of the left wheel in meters
        /// </summary>
        [DataMember]
        private double leftWheelMean;

        /// <summary>
        /// Absolute distance travelled of the right wheel in meters
        /// </summary>
        [DataMember]
        private double rightWheelAbsolute;

        /// <summary>
        /// Absolute  distance travelled of the left wheel in meters
        /// </summary>
        [DataMember]
        private double leftWheelAbsolute;

        /// <summary>
        /// Absolute  heading in radians.
        /// </summary>
        [DataMember]
        private double headingAbsolute;

        /// <summary>
        /// Current heading in radians
        /// </summary>
        [DataMember]
        private double? heading;

        /// <summary>
        /// Odometry noise model params.
        /// </summary>
        [DataMember]
        private OdometryNoiseModelParams noiseParams;

        /// <summary>
        /// Absolute position of the robot in x,y, theta. This is used for making sure that if messages are dropped, we still get the 
        /// correct delta x, delta y, delta heading.
        /// Nominal Position would be the absolute position of the robot after applying heading correction using external sensor like gyro.
        /// </summary>
        [DataMember]
        private Pose2D nominalPosition;

        /// <summary>
        /// Nominal heading.
        /// This is the absolute heading as reported by the external sensor like gyroscope after it is corrected by wheel odometry.
        /// This is useful for handling dropped messages. 
        /// </summary>
        [DataMember]
        private double nominalHeading;

        /// <summary>
        /// Previous drive feedback state.
        /// </summary>
        [DataMember]
        private DriveFeedbackState previousNominalState;

        /// <summary>
        /// Initializes a new instance of the <see cref="DriveFeedbackState"/> class.
        /// </summary>
        /// <param name="leftWheelMean">Left wheel mean in meters</param>
        /// <param name="rightWheelMean">Right wheel mean in meters</param>
        /// <param name="wheelbase">Wheel base in meters</param>
        /// <param name="noiseParams">Odometry noise parameters</param>
        /// <param name="previousNominalState">Previous nominal drive feedback state</param>
        public DriveFeedbackState(double leftWheelMean, double rightWheelMean, double wheelbase, OdometryNoiseModelParams noiseParams, DriveFeedbackState previousNominalState)
            : this(wheelbase, noiseParams)
        {
            this.leftWheelMean = leftWheelMean;
            this.rightWheelMean = rightWheelMean;
            this.Initialize(previousNominalState);
        }

        /// <summary>
        ///  Initializes a new instance of the <see cref="DriveFeedbackState"/> class.
        /// </summary>
        /// <param name="leftWheelMean">Left wheel mean in meters</param>
        /// <param name="rightWheelMean">Right wheel mean in meters</param>
        /// <param name="headingMean">Heading mean in radians</param>
        /// <param name="wheelbase">Wheel base in meters</param>
        /// <param name="noiseParams">Odometry noise parameters</param>
        /// <param name="previousNominalState">Nominal previous drive state</param>
        public DriveFeedbackState(double leftWheelMean, double rightWheelMean, double headingMean, double wheelbase, OdometryNoiseModelParams noiseParams, DriveFeedbackState previousNominalState)
            : this(wheelbase, noiseParams)
        {
            this.leftWheelMean = leftWheelMean;
            this.rightWheelMean = rightWheelMean;
            this.heading = headingMean;
            this.Initialize(previousNominalState);
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="DriveFeedbackState"/> class.
        /// </summary>
        /// <param name="that">Prototype for constructing this</param>
        public DriveFeedbackState(DriveFeedbackState that)
        {
            this.heading = that.heading;
            this.headingAbsolute = that.headingAbsolute;
            this.leftWheelAbsolute = that.leftWheelAbsolute;
            this.leftWheelMean = that.leftWheelMean;
            this.noiseParams = that.noiseParams;
            this.nominalHeading = that.nominalHeading;
            this.nominalPosition = that.nominalPosition;
            this.previousNominalState = that.previousNominalState;
            this.rightWheelAbsolute = that.rightWheelAbsolute;
            this.rightWheelMean = that.rightWheelMean;
            this.wheelbase = that.wheelbase;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="DriveFeedbackState"/> class.
        /// </summary>        
        /// <param name="wheelbase">Wheel base in meters</param>
        /// <param name="noiseParams">Odometry noise parameters</param>
        private DriveFeedbackState(double wheelbase, OdometryNoiseModelParams noiseParams)
        {
            this.wheelbase = wheelbase;
            this.noiseParams = noiseParams;
        }

        /// <summary>
        /// Gets the previous nominal state
        /// </summary>
        internal DriveFeedbackState PreviousNominalState 
        { 
            get 
            { 
                return this.previousNominalState; 
            }
        }

        /// <summary>
        /// Interpolates the between the start and end states for the desiredTime.
        /// </summary>
        /// <param name="startTime">The start time for interpolation</param>
        /// <param name="endTime">The end time for interpolation</param>
        /// <param name="desiredTime">Desired time for interpolation. This must be between (inclusive) start and end times</param>
        /// <param name="start">Start value</param>
        /// <param name="end">End value</param>    
        /// <returns>The linear interpolated value at the desired time</returns>
        public static DriveFeedbackState Interpolate(long startTime, long endTime, long desiredTime, DriveFeedbackState start, DriveFeedbackState end)
        {
            DriveFeedbackState interpolated = new DriveFeedbackState(start);
            if (start.heading != null && end.heading != null)
            {
                interpolated.heading = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTime, start.heading.Value, end.heading.Value);
            }
            else
            {
                interpolated.heading = null;
            }

            interpolated.headingAbsolute = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTime, start.headingAbsolute, end.headingAbsolute);
            interpolated.leftWheelAbsolute = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.leftWheelAbsolute, end.leftWheelAbsolute);
            interpolated.rightWheelAbsolute = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.rightWheelAbsolute, end.rightWheelAbsolute);
            interpolated.leftWheelMean = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.leftWheelMean, end.leftWheelMean);
            interpolated.rightWheelMean = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.rightWheelMean, end.rightWheelMean);

            return interpolated;
        }

        /// <summary>
        /// Creates a drive feedback state for the given updated heading calculated using other means (gyro / slam / graph optimizer etc).
        /// </summary>
        /// <param name="headingMean">Updated mean heading of the robot</param>        
        /// <returns>Drive feedback state for the given current pose</returns>
        public DriveFeedbackState CreateDriveFeedbackState(double headingMean)
        {
            return new DriveFeedbackState(this.leftWheelMean, this.rightWheelMean, headingMean, this.wheelbase, this.noiseParams, this.previousNominalState);
        }

        /// <summary>
        /// Gets the mean delta position (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetMeanDeltaPose(IDriveState previousState)
        {
            double fitness;
            return this.GetMeanDeltaPose(previousState, out fitness);
        }

        /// <summary>
        /// Gets the mean delta position (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetMeanDeltaPose(IDriveState previousState, out double fitness)
        {
            DriveFeedbackState previousDriveFeedbackState = previousState as DriveFeedbackState;
            if (previousDriveFeedbackState == null)
            {
                throw new System.ArgumentException(string.Format("Previous drive feeback type {0} is different from current drive feedback type {1}", previousState.GetType(), this.GetType()));
            }

            return this.GetDeltaPoseWithNoise(previousDriveFeedbackState, false, out fitness);
        }

        /// <summary>
        /// Samples the delta pose (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>        
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SampleDeltaPose(IDriveState previousState)
        {
            double fitness;
            return this.SampleDeltaPose(previousState, out fitness);
        }

        /// <summary>
        /// Samples the delta pose (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SampleDeltaPose(IDriveState previousState, out double fitness)
        {
            DriveFeedbackState previousDriveFeedbackState = previousState as DriveFeedbackState;
            if (previousDriveFeedbackState == null)
            {
                throw new System.ArgumentException(string.Format("Previous drive feeback type {0} is different from current drive feedback type {1}", previousState.GetType(), this.GetType()));
            }

            return this.GetDeltaPoseWithNoise(previousDriveFeedbackState, !this.noiseParams.IgnoreNoiseParams, out fitness);
        }

        /// <summary>
        /// Gets the updated pose (X, Y, Heading) from the given  previous pose and previous state.
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>        
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetUpdatedPose(Pose2D previousPose, IDriveState previousState)
        {
            double fitness;
            return this.GetUpdatedPose(previousPose, previousState, out fitness);
        }

        /// <summary>
        /// Gets the updated pose (X, Y, Heading) from the given  previous pose and previous state.
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetUpdatedPose(Pose2D previousPose, IDriveState previousState, out double fitness)
        {
            Pose2D deltaPose = this.GetMeanDeltaPose(previousState, out fitness);

            CoordinateTransform2D coordTranform = new CoordinateTransform2D(previousPose);
            Pose2D updatedPose = coordTranform.Transform(deltaPose);
            
            ////Tracing.TraceOut.Info(Tracing.TraceContexts.Nav, "Mean Updated pose {0} delta pose {1}", updatedPose, deltaPose);
            return updatedPose;
        }

        /// <summary>
        /// Samples the pose (X, Y, Heading) from the given previous state
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>        
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SamplePose(Pose2D previousPose, IDriveState previousState)
        {
            double fitness;
            return this.SamplePose(previousPose, previousState, out fitness);
        }

        /// <summary>
        /// Samples the pose (X, Y, Heading) from the given previous state
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SamplePose(Pose2D previousPose, IDriveState previousState, out double fitness)
        {
            Pose2D deltaPose = this.SampleDeltaPose(previousState, out fitness);
            
            CoordinateTransform2D coordTranform = new CoordinateTransform2D(previousPose);
            Pose2D updatedPose = coordTranform.Transform(deltaPose);
            ////Tracing.TraceOut.Info(Tracing.TraceContexts.Nav, "Sample Updated pose {0} delta pose {1}", updatedPose, deltaPose);

            return updatedPose;
        }

        /// <summary>
        /// Initializes the nominal position for the drive feedback state.
        /// </summary>
        /// <param name="previousNominalState">Previous drive feedback state</param>
        private void Initialize(DriveFeedbackState previousNominalState)
        {
            if (previousNominalState != null)
            {
                this.previousNominalState = new DriveFeedbackState(previousNominalState.wheelbase, previousNominalState.noiseParams);
                this.previousNominalState.leftWheelMean = previousNominalState.leftWheelMean;
                this.previousNominalState.rightWheelMean = previousNominalState.rightWheelMean;
                this.previousNominalState.heading = previousNominalState.heading;
                this.previousNominalState.nominalHeading = previousNominalState.nominalHeading;
                this.previousNominalState.nominalPosition = previousNominalState.nominalPosition;
            }
            else
            {
                this.previousNominalState = new DriveFeedbackState(this.wheelbase, new OdometryNoiseModelParams());
                this.previousNominalState.leftWheelMean = this.leftWheelMean;
                this.previousNominalState.rightWheelMean = this.rightWheelMean;
                this.previousNominalState.heading = this.heading;
                this.previousNominalState.nominalHeading = this.nominalHeading;
                this.previousNominalState.nominalPosition = this.nominalPosition;
            }

            Pose2D previousNominalPosition = this.previousNominalState.nominalPosition;
            double previousHeading = 0;
            double previousLeftWheelMean = this.previousNominalState.leftWheelMean;
            double previousRightWheelMean = this.previousNominalState.rightWheelMean;
            if (this.previousNominalState.heading != null)
            {
                previousHeading = this.previousNominalState.heading.Value;
            }

            double? externalHeadingDelta = this.GetExternalHeadingDelta(this.previousNominalState);

            double deltaLeftWheel = (this.leftWheelMean - previousLeftWheelMean);
            double deltaRightWheel = (this.rightWheelMean - previousRightWheelMean);
            double fitness;
            double deltaHeading = previousNominalState == null ? 0 : this.CalculateHeadingDelta(this.previousNominalState, false, deltaLeftWheel, deltaRightWheel, externalHeadingDelta, out fitness);
            double meanHeading = previousHeading + deltaHeading * 0.5;
            double deltaX = (deltaLeftWheel + deltaRightWheel) * 0.5 * Math.Cos(meanHeading);
            double deltaY = (deltaLeftWheel + deltaRightWheel) * 0.5 * Math.Sin(meanHeading);

            this.nominalPosition = previousNominalPosition + new Pose2D(deltaX, deltaY, deltaHeading);
            this.nominalHeading = previousNominalState == null ? 0 : previousNominalState.nominalHeading + deltaHeading;
            this.leftWheelAbsolute = this.previousNominalState.leftWheelAbsolute + Math.Abs(this.leftWheelMean - this.previousNominalState.leftWheelMean);
            this.rightWheelAbsolute = this.previousNominalState.rightWheelAbsolute + Math.Abs(this.rightWheelMean - this.previousNominalState.rightWheelMean);
            this.headingAbsolute = this.previousNominalState.headingAbsolute + Math.Abs(deltaHeading);
        }

        /// <summary>
        /// Gets the external heading delta, which can be null if the current or previous heading is not populated.
        /// </summary>
        /// <param name="previousState">Previous drive feedback state</param>
        /// <returns>External (Gyro) heading delta</returns>
        private double? GetExternalHeadingDelta(DriveFeedbackState previousState)
        {
            double? externalHeadingDelta = null;
            if (this.heading != null && previousState.heading != null)
            {
                externalHeadingDelta = this.heading - previousState.heading;
            }

            return externalHeadingDelta;
        }

        /// <summary>
        /// Gets the delta pose after applying noise if specified.
        /// </summary>
        /// <param name="previousState">The previous drive feedback state</param>        
        /// <param name="applyNoise">Indicates whether to apply the noise model</param>
        /// <param name="fitness">The fitness between the wheel heading delta and the external sensor (for example gyro) heading delta
        /// This is NaN if there is no external sensor information</param>        
        /// <returns>The position delta from previous pose</returns>
        private Pose2D GetDeltaPoseWithNoise(DriveFeedbackState previousState, bool applyNoise, out double fitness)
        {
            fitness = double.NaN;
            Pose2D poseDelta;
            double deltaX = this.nominalPosition.X - previousState.nominalPosition.X;
            double deltaY = this.nominalPosition.Y - previousState.nominalPosition.Y;
            double leftNoise = 0;
            double rightNoise = 0;

            // Calculate the noise and stored it. The same noise is used later.            
            if (applyNoise)
            {
                leftNoise = this.CalculateLeftWheelNoise(this.leftWheelAbsolute - previousState.leftWheelAbsolute);
                rightNoise = this.CalculateRightWheelNoise(this.rightWheelAbsolute - previousState.rightWheelAbsolute);
            }

            // Calculate the heading delta with noise
            double deltaHeading = this.CalculateHeadingDelta(previousState, applyNoise, this.leftWheelMean - previousState.leftWheelMean + leftNoise, this.rightWheelMean - previousState.rightWheelMean + rightNoise, this.GetExternalHeadingDelta(previousState), out fitness);

            // Calculate the mean heading without noise
            double deltaHeadingMean = this.CalculateHeadingDelta(previousState, false, this.leftWheelMean - previousState.leftWheelMean, this.rightWheelMean - previousState.rightWheelMean, this.GetExternalHeadingDelta(previousState), out fitness);

            // The arc model assumption of applying half the mean heading delta breaks down in case of small deltaX or deltaY 
            // In those cases apply the translation model.
            // Also note that in case of small deltaX, applying arc model can result in singularities since it has an Atan2(deltaY, deltaX) component.
            const double SmallTranslationInMeters = 0.001;
            bool applyTranslationModel = Math.Abs(deltaX) < SmallTranslationInMeters || Math.Abs(deltaY) < SmallTranslationInMeters;
            
            double deltaRightAbsolute = this.rightWheelAbsolute - previousState.rightWheelAbsolute;
            double deltaLeftAbsolute = this.leftWheelAbsolute - previousState.leftWheelAbsolute;

            // Apply initial and final angle rotations.            
            // This is required to account for any heading errors since arc model cannot allow for arbitrary initial and final headings.
            double initialToArcSegmentAngle = applyTranslationModel ? 0 : Pose2D.SmallestCircularDeltaAngle(Math.Atan2(deltaY, deltaX), (deltaHeadingMean) / 2);            
           
            // The final angle is the inverse of the initial heading, this comes from the arc model maths.
            double arcSegmentToFinalAngle = -initialToArcSegmentAngle;                       

            double deltaHeadingForTranslation;            
            
#if USE_HEADING_WITH_WHEEL_NOISE
                deltaHeadingForTranslation = deltaHeading;
#else
                deltaHeadingForTranslation = deltaHeadingMean;
#endif

            // Assume that when the robot travels in the arc, the mean delta heading is half the overall delta heading.
            double deltaRightInitial = initialToArcSegmentAngle * this.wheelbase * 0.5;
            double deltaLeftInitial = -deltaRightInitial;
            double previousHeading = 0;

            // ReCalculate the delta translation from stored delta x, this handles dropped intermediate messages.
            double deltaRightTranslation;
            double deltaLeftTranslation;

            // To avoid numeric stability issues, determine whether the robot is moving sideways and use the appropriate model
            bool sidewaysMotion = Math.Abs(Math.Cos(previousHeading + initialToArcSegmentAngle + deltaHeadingForTranslation / 2)) < 0.001;
            if (!sidewaysMotion)
            {
                deltaRightTranslation = deltaX / Math.Cos(previousHeading + initialToArcSegmentAngle + deltaHeadingForTranslation / 2) + deltaHeadingForTranslation * this.wheelbase * 0.5;
                deltaLeftTranslation = deltaX / Math.Cos(previousHeading + initialToArcSegmentAngle + deltaHeadingForTranslation / 2) - deltaHeadingForTranslation * this.wheelbase * 0.5;
            }
            else
            {
                deltaRightTranslation = deltaY / Math.Sin(previousHeading + initialToArcSegmentAngle + deltaHeadingForTranslation / 2) + deltaHeadingForTranslation * this.wheelbase * 0.5;
                deltaLeftTranslation = deltaY / Math.Sin(previousHeading + initialToArcSegmentAngle + deltaHeadingForTranslation / 2) - deltaHeadingForTranslation * this.wheelbase * 0.5;
            }

            // Reapply the noise on the recalculated translations. Note that it is a two step process.           
            // This is tricky. 
            // We need to apply combine the wheel and gyro data to create a corrected mean heading.           
            // Based on this mean heading we calculate a corrected pose.
            // Using this corrected pose we re calculate the wheel motion which would have resulted in this pose and heading.
            // Then we apply the same amount wheel noise to this corrected motion to get the noisy motion as we apply to the original
            // wheels to get the noisy heading.
            // Note that the wheel noise is NOT applied twice.
            if (applyNoise)
            {
                deltaRightTranslation += rightNoise;
                deltaLeftTranslation += leftNoise;
            }

            double deltaRightFinal = arcSegmentToFinalAngle * this.wheelbase * 0.5;
            double deltaLeftFinal = -deltaRightFinal;

            double deltaHeadingInitialWithNoise = (deltaRightInitial - deltaLeftInitial) / this.wheelbase;
            double deltaHeadingWithNoise = deltaHeading;
            double deltaHeadingFinalWithNoise = (deltaRightFinal - deltaLeftFinal) / this.wheelbase;
            double meanHeadingWithNoise = previousHeading + deltaHeadingWithNoise * 0.5;
            double translationDistanceWithNoise = (deltaLeftTranslation + deltaRightTranslation) * 0.5;
            double deltaXWithNoise = translationDistanceWithNoise * Math.Cos(meanHeadingWithNoise);
            double deltaYWithNoise = translationDistanceWithNoise * Math.Sin(meanHeadingWithNoise);

            poseDelta = new Pose2D(deltaXWithNoise, deltaYWithNoise, deltaHeadingWithNoise);
            return poseDelta;
        }

        /// <summary>
        /// Calculates the heading delta between this drive feedback state and previous drive feedback state.
        /// </summary>
        /// <param name="previousState">Previous state</param>
        /// <param name="applyNoise">Determines whether to apply noise</param>
        /// <param name="leftDelta">Left wheel delta in meters</param>
        /// <param name="rightDelta">Right wheel delta in meters</param>
        /// <param name="externalHeadingDelta">External heading delta in radians</param>
        /// <param name="fitness">Fitness between wheel odometry heading and external heading</param>
        /// <returns>Heading delta</returns>
        private double CalculateHeadingDelta(DriveFeedbackState previousState, bool applyNoise, double leftDelta, double rightDelta, double? externalHeadingDelta, out double fitness)
        {
            double headingDelta;
            double odometryHeadingDelta = (rightDelta - leftDelta) / this.wheelbase;
            if (externalHeadingDelta != null)
            {
                double externalHeadingAbsoluteDelta = this.nominalHeading - previousState.nominalHeading;
                if (applyNoise)
                {
                    externalHeadingDelta += this.CalculateHeadingNoise(externalHeadingAbsoluteDelta);
                }

                double odometryDeltaStdDev = Math.Sqrt(this.noiseParams.LeftWheelStdDevInMeters * this.noiseParams.LeftWheelStdDevInMeters + this.noiseParams.RightWheelStdDevInMeters * this.noiseParams.RightWheelStdDevInMeters) / this.wheelbase;
                double combinedStdDev = Math.Sqrt(odometryDeltaStdDev * odometryDeltaStdDev + this.noiseParams.HeadingStdDevInRadians * this.noiseParams.HeadingStdDevInRadians);
                if (odometryDeltaStdDev == 0 && this.noiseParams.HeadingStdDevInRadians != 0)
                {
                    headingDelta = odometryHeadingDelta;
                }
                else if (this.noiseParams.HeadingStdDevInRadians == 0 && odometryDeltaStdDev != 0)
                {
                    headingDelta = externalHeadingDelta.Value;
                }
                else
                {
                    //// headingDelta = (odometryHeadingDelta + externalHeadingDelta) / 2;
                    double stdSum = odometryDeltaStdDev + this.noiseParams.HeadingStdDevInRadians;
                    if (stdSum != 0)
                    {
                        headingDelta = (odometryHeadingDelta * this.noiseParams.HeadingStdDevInRadians + externalHeadingDelta.Value * odometryDeltaStdDev) / stdSum;
                    }
                    else
                    {
                        headingDelta = (odometryHeadingDelta + externalHeadingDelta.Value) * 0.5;
                    }
                }

                fitness = Math.Log(1 - Math.Abs(Pose2D.SmallestCircularDeltaAngle(odometryHeadingDelta, externalHeadingDelta.Value)) / Math.PI);                    
            }
            else
            {
                headingDelta = odometryHeadingDelta;
                fitness = double.NaN;             
            }

            return headingDelta;
        }

        /// <summary>
        /// Calculates a error distribution resulting from left wheel motion
        /// </summary>
        /// <param name="translationalDistanceMeters">The distance of the translation</param>
        /// <returns>A Gaussian that contains the distribution</returns>
        private double CalculateLeftWheelNoise(double translationalDistanceMeters)
        {
            return
                new Gaussian(
                    0,
                    Math.Pow(this.noiseParams.LeftWheelStdDevInMeters, 2)
                    * Math.Abs(translationalDistanceMeters)).Sample();
        }

        /// <summary>
        /// Calculates a error distribution resulting from right wheel motion
        /// </summary>
        /// <param name="translationalDistanceMeters">The distance of the translation</param>
        /// <returns>A Gaussian that contains the distribution</returns>
        private double CalculateRightWheelNoise(double translationalDistanceMeters)
        {
            return
                new Gaussian(
                    0,
                    Math.Pow(this.noiseParams.RightWheelStdDevInMeters, 2)
                    * Math.Abs(translationalDistanceMeters)).Sample();
        }

        /// <summary>
        /// Calculates a heading error distribution from a rotation
        /// </summary>
        /// <param name="rotationInRads">The size of the rotation in radians</param>
        /// <returns>A Gaussian that contains the distribution</returns>
        private double CalculateHeadingNoise(double rotationInRads)
        {
            return
                new Gaussian(
                    0,
                    Math.Pow(this.noiseParams.HeadingStdDevInRadians, 2)
                    * Math.Abs(rotationInRads)).Sample();
        }
    }
}
